package com.hongji.lms5xxcommunicator.realtime.sync;

/** 时间戳(hostNs)-X 的环形缓冲，支持最近邻查询与局部速度估计。 */
public final class XRingBuffer {
    private final long[] t;
    private final double[] x;
    private int head = -1;
    private final int mask;

    public XRingBuffer(int capacity) {
        int n = 1; while (n < capacity) n <<= 1; // 扩为2次幂
        t = new long[n]; x = new double[n]; mask = n - 1;
    }

    public void add(long hostNs, double xv) {
        int i = (head + 1) & mask;
        t[i] = hostNs; x[i] = xv; head = i;
    }

    /** 在容差内(±tolNs)找最近X；未找到返回 NaN。 */
    public double nearest(long hostNs, long tolNs) {
        int h = head; if (h < 0) return Double.NaN;
        double bestX = Double.NaN; long bestDt = Long.MAX_VALUE;
        for (int k = 0; k <= mask; k++) {
            int idx = (h - k) & mask;
            long tk = t[idx]; if (tk == 0L) break;
            long dt = Math.abs(hostNs - tk);
            if (dt < bestDt) { bestDt = dt; bestX = x[idx]; }
            if (tk < hostNs - tolNs && bestDt < tolNs / 4) break;
        }
        return (bestDt <= tolNs) ? bestX : Double.NaN;
    }

    /** 局部速度估计（单位与X/时间一致；windowNs建议50ms左右）。 */
    public double velocity(long hostNs, long windowNs) {
        int h = head; if (h < 1) return 0.0;
        long left = hostNs - windowNs, right = hostNs + windowNs;
        double xl = Double.NaN, xr = Double.NaN; long tl = 0, tr = 0;

        for (int k = 0; k <= mask; k++) {
            int idx = (h - k) & mask;
            long tk = t[idx]; if (tk == 0L) break;
            if (tk >= left && tk <= hostNs) { tl = tk; xl = x[idx]; }
            if (tk <= right && tk >= hostNs) { tr = tk; xr = x[idx]; }
            if (tl != 0 && tr != 0) break;
        }
        if (Double.isNaN(xl) || Double.isNaN(xr) || tl == tr) return 0.0;
        return (xr - xl) / ((tr - tl) * 1e-9); // X单位为mm则mm/s
    }
}
